
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_commands.c
  * @author     baiyang
  * @date       2022-10-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// checks if we should update ahrs/RTL home position from the EKF
void fms_update_home_from_EKF()
{
    // exit immediately if home already set
    if (ahrs_home_is_set(fms.ahrs)) {
        return;
    }

    // special logic if home is set in-flight
    if (fms.motors->_armed) {
        fms_set_home_to_current_location_inflight();
    } else {
        // move home to current ekf location (this will set home_state to HOME_SET)
        if (!fms_set_home_to_current_location(false)) {
            // ignore failure
        }
    }
}

// set_home - sets ahrs home (used for RTL) to specified location
//  initialises inertial nav and compass on first call
//  returns true if home location set successfully
bool fms_set_home(const Location* loc, bool lock)
{
    // check EKF origin has been set
    Location ekf_origin;
    if (!ahrs_get_origin(fms.ahrs, &ekf_origin)) {
        return false;
    }

    // check home is close to EKF origin
    if (fms_far_from_EKF_origin(loc)) {
        return false;
    }

    const bool home_was_set = ahrs_home_is_set(fms.ahrs);

    // set ahrs home (used for RTL)
    if (!ahrs_set_home(fms.ahrs, loc)) {
        return false;
    }

    // init inav and compass declination
    if (!home_was_set) {
#if MODE_AUTO_ENABLED == ENABLED
        // log new home position which mission library will pull from ahrs
        struct Mission_Command temp_cmd;
        if (mission_read_cmd_from_storage(0, &temp_cmd)) {
            aplog_write_mission_cmd(&temp_cmd);
        }
#endif
    }

    // lock home position
    if (lock) {
        ahrs_lock_home(fms.ahrs);
    }

    // return success
    return true;
}

// far_from_EKF_origin - checks if a location is too far from the EKF origin
//  returns true if too far
bool fms_far_from_EKF_origin(const Location* loc)
{
    // check distance to EKF origin
    Location ekf_origin;
    if (ahrs_get_origin(fms.ahrs, &ekf_origin)) {
        if (labs(ekf_origin.alt - loc->alt)*0.01f > EKF_ORIGIN_MAX_ALT_KM*1000.0f) {
            return true;
        }
    }

    // close enough to origin
    return false;
}

// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void fms_set_home_to_current_location_inflight()
{
    // get current location from EKF
    Location temp_loc;
    Location ekf_origin;
    if (ahrs_get_location(fms.ahrs, &temp_loc) && ahrs_get_origin(fms.ahrs, &ekf_origin)) {
        temp_loc.alt = ekf_origin.alt;
        if (!fms_set_home(&temp_loc, false)) {
            return;
        }
        // we have successfully set AHRS home, set it for SmartRTL
#if MODE_SMARTRTL_ENABLED == ENABLED
        //g2.smart_rtl.set_home(true);
#endif
    }
}

// set_home_to_current_location - set home to current GPS location
bool fms_set_home_to_current_location(bool lock)
{
    // get current location from EKF
    Location temp_loc;
    if (ahrs_get_location(fms.ahrs, &temp_loc)) {
        if (!fms_set_home(&temp_loc, lock)) {
            return false;
        }
        // we have successfully set AHRS home, set it for SmartRTL
#if MODE_SMARTRTL_ENABLED == ENABLED
        //g2.smart_rtl.set_home(true);
#endif
        return true;
    }
    return false;
}

/*------------------------------------test------------------------------------*/


